#include "pid.h"
#include <stdio.h>

void PID_Init(PIDTypeDef_t *pid, float Kp, float Ki, float Kd, float setPoint)
{
    if (pid == NULL)
    {
        return;
    }

    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->Kp_out = 0;
    pid->Ki_out = 0;
    pid->Kd_out = 0;
    pid->last_Kd_out = 0;
    pid->error = 0.0f;
    pid->setPoint = setPoint;
    pid->lastError = 0.0f;
    pid->Err_Sum = 0.0f;
    pid->output = 0.0f;
    pid->beta = 1;
}

void PID_Set(PIDTypeDef_t *pid, float Kp, float Ki, float Kd)
{
    if (pid == NULL)
    {
        return;
    }

    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
}

float PID_Update(PIDTypeDef_t *pid, float feedback)
{
    if (pid == NULL)
    {
        return 0.0f;
    }

    pid->error = pid->setPoint - feedback;

    pid->Err_Sum += pid->error;
    LIMIT_MIN_MAX(pid->Err_Sum, -2000 / pid->Ki, 2000.0f / pid->Ki);
    pid->Kp_out = pid->Kp * pid->error;


    pid->Ki_out = pid->Ki * pid->Err_Sum;
    pid->Kd_out = pid->Kd * (pid->error - pid->lastError);


    pid->last_Kd_out = pid->Kd_out;

    LIMIT_MIN_MAX(pid->Ki_out, -2000, 2000);



    pid->output = pid->Kp_out + pid->Ki_out + pid->Kd_out;

    LIMIT_MIN_MAX(pid->output, -3100, 3100);

    pid->lastError = pid->error;

    return pid->output;
}

float PID_Update_IMU(PIDTypeDef_t *pid, float feedback)
{
    if (pid == NULL)
    {
        return 0.0f;
    }
    
    pid->error = pid->setPoint - feedback;
    if (pid->error > 180.0f)
    {
        pid->error -= 360.0f;
    }
    else if (pid->error < -180.0f)
    {
        pid->error+= 360.0f;
    }
    pid->Err_Sum += pid->error;
    LIMIT_MIN_MAX(pid->Err_Sum, -2000 / pid->Ki, 2000.0f / pid->Ki);
    pid->Kp_out = pid->Kp * pid->error;


    pid->Ki_out = pid->Ki * pid->Err_Sum;
    pid->Kd_out = pid->Kd * (pid->error - pid->lastError);

    pid->Kd_out = pid->Kd_out * 0.8f + pid->last_Kd_out * 0.2f;
    pid->last_Kd_out = pid->Kd_out;

    LIMIT_MIN_MAX(pid->Ki_out, -2000, 2000);

//    if(pid->Ki_out > 2000 || pid->Ki_out < -2000)
//    {
//        pid->Err_Sum = pid->Err_Sum * 0.7f;
//    }

    pid->output = pid->Kp_out + pid->Ki_out + pid->Kd_out;

    LIMIT_MIN_MAX(pid->output, -3100, 3100);

    pid->lastError = pid->error;

    return pid->output;
}

void setPoint(PIDTypeDef_t *pid, float point)
{
    if (pid == NULL)
    {
        return;
    }

    pid ->setPoint = point;
}
